#include "pid.h"
#include <stdint.h>
#include <stdio.h>

uint8_t inAuto;

//Actual tuning parameters used in PID calculation.
volatile static float Kc_;
volatile static float tauR_;
volatile static float tauD_;

//The point we want to reach.
volatile static float setPoint_;
//The thing we measure.
volatile static float processVariable_;
volatile static float prevProcessVariable_;
//The output that affects the process variable.
volatile static float controllerOutput_;
volatile static float prevControllerOutput_;

//We work in % for calculations so these will scale from
//real world values to 0-100% and back again.
volatile static float inMin_;
volatile static float inMax_;
volatile static float inSpan_;
volatile static float outMin_;
volatile static float outMax_;
volatile static float outSpan_;

//The accumulated error, i.e. integral.
volatile static float accError_;
//The controller output bias.
volatile static float bias_;

//The interval between samples.
volatile static float tSample_ = 1;

//Controller output as a real world value.
volatile float realOutput_;


float PID_Limit(float val)
{
    int64_t temp = 0;
    
    val *= 1000;
    temp = val;
    val = temp;
    val /= 1000.0;

    return val;
}

void PID_constants(float Kc, float tauI, float tauD, float interval, float min_output_limit)
{
    inAuto = 1;

    //Default the limits to the full range of I/O: 3.3V
    //Make sure to set these to more appropriate limits for
    //your application.
    tSample_ = interval / inSpan_;

    //setTunings(Kc, tauI, tauD);
    Kc_ = Kc;
    tauR_ = tauI;
    tauD_ = tauD;

    setPoint_ = 0.0;
    processVariable_ = 0.0;
    prevProcessVariable_ = 0.0;
    controllerOutput_ = 0.0;
    prevControllerOutput_ = 0.0;

    if(accError_ < 0) accError_ = 0;
    bias_ = 0.0;

    realOutput_ = 0.0;
}

void setInputLimits(float inMin, float inMax)
{
    //Make sure we haven't been given impossible values.
    if (inMin >= inMax)
    {
        return;
    }
    //Rescale the working variables to reflect the changes.
    prevProcessVariable_ *= (inMax - inMin) / inSpan_;
    // accError_ *= (inMax - inMin) / inSpan_;
    //Make sure the working variables are within the new limits.
    if (prevProcessVariable_ > 1)
    {
        prevProcessVariable_ = 1;
    }
    else if (prevProcessVariable_ < 0)
    {
        prevProcessVariable_ = 0;
    }
    inMin_ = inMin;
    inMax_ = inMax;
    inSpan_ = inMax - inMin;
}

void setOutputLimits(float outMin, float outMax)
{
    //Make sure we haven't been given impossible values.
    if (outMin >= outMax)
    {
        return;
    }
    //Rescale the working variables to reflect the changes.
    prevControllerOutput_ *= (outMax - outMin) / outSpan_;
    //Make sure the working variables are within the new limits.
    if (prevControllerOutput_ > 1)
    {
        prevControllerOutput_ = 1;
    }
    else if (prevControllerOutput_ < 0)
    {
        prevControllerOutput_ = 0;
    }
    outMin_ = outMin;
    outMax_ = outMax;
    outSpan_ = outMax - outMin;
}

void setTunings(float Kc, float tauI, float tauD)
{
    // //Verify that the tunings make sense.
    // if (Kc == 0.0 || tauI < 0.0 || tauD < 0.0)
    // {
    //     return;
    // }

    // float tempTauR;

    // if (tauI == 0.0)
    // {
    //     tempTauR = 0.0;
    // }
    // else
    // {
    //     tempTauR = (1.0 / tauI) * tSample_;
    // }
    // //For "bumpless transfer" we need to rescale the accumulated error.
    // if (inAuto == 1)
    // {
    //     if (tempTauR == 0.0)
    //     {
    //         accError_ = 0.0;
    //     }
    //     else
    //     {
    //         accError_ *= (Kc_ * tauR_) / (Kc * tempTauR);
    //     }
    // }

    // Kc_ = Kc;
    // tauR_ = tempTauR;
    // tauD_ = tauD / tSample_;
    Kc_ = Kc;
    tauR_ = tauI;
    tauD_ = tauD;
}

void PID_reset(void)
{
    float scaledBias = 0.0;

    scaledBias = (realOutput_ - outMin_) / outSpan_;
    prevControllerOutput_ = scaledBias;
    prevProcessVariable_ = 0; //(processVariable_ - inMin_) / inSpan_;
    //Clear any error in the integral.
    accError_ /= 1.10;
}

void setMode(int mode)
{
    //We were in manual, and we just got set to auto.
    //Reset the controller internals.
    if (mode != 0 && !inAuto)
    {
        PID_reset();
    }
    inAuto = (mode != 0);
}

void setInterval(float interval)
{
    if (interval > 0)
    {
        //Convert the time-based tunings to reflect this change.
        tauR_ *= (interval / tSample_);
        // accError_ *= (tSample_ / interval);
        tauD_ *= (interval / tSample_);
        tSample_ = interval;
    }
}

void PID_setSetPoint(float sp)
{
    setPoint_ = sp;
}

void PID_setProcessValue(float pv)
{
    processVariable_ = pv;
}

float PID_compute()
{
		float scaledSP;
	  float error;
	  float dMeas;
	  float scaledPV;

    //Pull in the input and setpoint, and scale them into percent span.
    scaledPV = (processVariable_ - inMin_) / inSpan_;
    if (scaledPV > 1.0)
    {
        scaledPV = 1.0;
    }
    else if (scaledPV < 0.0)
    {
        scaledPV = 0.0;
    }

    scaledSP = (setPoint_ - inMin_) / inSpan_;
    if (scaledSP > 1.0)
    {
        scaledSP = 1;
    }
    else if (scaledSP < 0.0)
    {
        scaledSP = 0;
    }

    error = scaledSP - scaledPV;
    //Check and see if the output is pegged at a limit and only
    //integrate if it is not. This is to prevent reset-windup.
    if (!((prevControllerOutput_ >= 1) && (error > 0)) && !((prevControllerOutput_ <= 0) && (error < 0)))
    {
        accError_ += error;
    }
    //Compute the current slope of the input signal.
    // float dMeas = (scaledPV - prevProcessVariable_) / tSample_;
    dMeas = (error - prevProcessVariable_);
    dMeas = PID_Limit(dMeas);
    //Perform the PID calculation.
    controllerOutput_ = (Kc_ * error) + (tauR_ * accError_) + (tauD_ * dMeas);
    // Temp_Param.probe2 = (tauD_ * dMeas);
    //Make sure the computed output is within output constraints.
    if (controllerOutput_ < 0.0)
    {
        controllerOutput_ = 0.0;
    }
    else if (controllerOutput_ > 1.0)
    {
        controllerOutput_ = 1.0;
    }
    
    //Remember this output for the windup check next time.
    prevControllerOutput_ = controllerOutput_;
    //Remember the input for the derivative calculation next time.
    prevProcessVariable_ = error;

    //Scale the output from percent span back out to a real world number.
    return ((controllerOutput_ * outSpan_) + outMin_);
}
